#include "OOPSMP/Core/StateSpace/Plugin/ArticulatedStateSpace.h"

namespace OOPSMP {

	/**
	 *@brief Macro for class, class pointer, and const class pointer
	 *       declarations 
	 */
	ForwardClassDeclaration(ArticulatedStateSpaceParams);

	/**
	 *@brief articulated state space params
	 *
	 */ 
	class ArticulatedStateSpaceParams : ArticulatedStateSpace {
		
		/**
		 *@brief Compute scaling factor so that state distance is in
		 *       <em>[0, 1]</em>
		 */
		double computeDistanceStateScalingFactor(void) 
		{
			OOPSMPconst_bbox_t  bbox   = getStateBoundingBox(); 
			const double       *coeffs = getDistanceStateCoeffs();
			ArticulatedPart_t ap	   = (ArticulatedPart_t)getCoreRobotData()->getWorkspace()->getMovingPart(getCoreRobotData()->getMovingPartId());   
			long n					   = ap->getStateVectorLength();
			double result			   = 0.0;
			long currentSVPosition	   = 7; // current state vector position - we need to manually take care of pos.7 and up
			int numLinks			   = ap->getLinkCount();

			// process XYZ the same as SE3StateSpace does
			result = sqrt( OOPSMPsquare(coeffs[0] * (bbox[1] - bbox[0])) +
						   OOPSMPsquare(coeffs[1] * (bbox[3] - bbox[2])) +
						   OOPSMPsquare(coeffs[2] * (bbox[5] - bbox[4])) ) + coeffs[3] * M_PI;

			// compute corresponding weights for each extra DoF from joints 1 onwards (state vector pos.7 and up)
			for (int i = 1; i < numLinks; i++) {

				int num_DOFs = ap->getNumDoFsForLink(i);

				// for each joint, get its DoF limits
				double * min = new double[num_DOFs];
				double * max = new double[num_DOFs];

				ap->getLinkJoint(i)->getJointLimits( min, max );

				// for each DoF in the current joint, add its maximum difference to the result
				for ( int j = 0 ; j < num_DOFs ; j++ ) {
					if ( ap->getLinkJoint(i)->isDoFAngle(j) )
						result += coeffs[currentSVPosition] * OOPSMPmax_distance_between_angles_in_interval( min[j], max[j] );
					else
						result += coeffs[currentSVPosition] * ( max[j] - min[j] );
				}

				currentSVPosition += 1;

				delete [] max;
				delete [] min;
			}

			return result;
		}

		/**
		 *@brief Compute state distance
		 *
		 *@param x1 state
		 *@param x2  state
		 */
		double distanceStates(State_const_t x1, State_const_t x2) const
		{
		    ArticulatedPart_t ap = (ArticulatedPart_t)getCoreRobotData()->getWorkspace()->getMovingPart(getCoreRobotData()->getMovingPartId());   
			const double *coeffs = getDistanceStateCoeffs();
			long n				 = ap->getStateVectorLength();
			double result		 = 0.0;
			long svPos			 = 7; // current state vector position - we need to manually take care of pos.7 and up

			// compute the XYZ distance for the tail, add quaternions (rotation) into the mix
			result = sqrt( OOPSMPsquare(coeffs[0] * (x1[0] - x2[0])) +
						   OOPSMPsquare(coeffs[1] * (x1[1] - x2[1])) +
						   OOPSMPsquare(coeffs[2] * (x1[2] - x2[2])) ) +
					 coeffs[3] * SO3Topology::getSingleton()->groupDistance(&(x1[3]), &(x2[3]));;
			
			// go over each link beyond tail
			for (int i = 1; i < ap->getLinkCount(); i++) {
				
				// go over each DoF in the current link's joint
				for ( int j = 0 ; j < ap->getNumDoFsForLink(i) ; j++ ) {
					
					// if DoF is an angle, use special function for it
					if ( ap->getLinkJoint(i)->isDoFAngle(j) )
						result += coeffs[svPos] * OOPSMPdistance_angles( x1[svPos], x2[svPos] );
					else
						result += coeffs[svPos] * ( x2[svPos] - x1[svPos] );

					// increment current state vector position
					svPos += 1;
				}
			}

			// finally scale the number into the state space
			result *= getDistanceStateScalingFactor();

			return result;
		}
	
	};

	/**
     *@brief Declare factory and specify in source file the functions that should
     *       be registered for plug-and-play functionality
     */
	DeclareInstanceFactory(ArticulatedStateSpaceParams, ArticulatedStateSpaceFactory);

}